#include "simple_bldcmotor.h"
#include "os/thread.h"
#include "soc/soc_misc.h"
#include <math.h>

#define SPWM_PHASE_DETA 120
#define SPWM_PHASE_TWO_DETA 240

#define SPWM_PHASE_INDEX_DETA 133
#define SPWM_PHASE_TWO_INDEX_DETA 267

#define DETA_IS_ZERO(deta)  (deta < 0.0001)

#define MOTOR_MIN_SPEED 0.002   //最小角速度
#define MOTOR_MAX_SPEED 3.8     //最大角速度

Simple_BLDC_Motor::Simple_BLDC_Motor()
{
}

Simple_BLDC_Motor::~Simple_BLDC_Motor()
{
    
}

void Simple_BLDC_Motor::Config(SOC_Timer *timera, int cha, 
                        SOC_Timer *timerb, int chb, 
                        SOC_Timer *timerc, int chc, 
                        int coilPair,
                        float ma,
                        SpwmSpeedMode mode)
{
    m_spwma.Config(timera, cha, ma);
    m_spwmb.Config(timerb, chb, ma);
    m_spwmc.Config(timerc, chc, ma);
    
    m_coilPair = coilPair;
    m_speedMode = mode;
}

void Simple_BLDC_Motor::ConfigSpeedAngle(float speedV0, float speedMax, float acc, int ms)
{
    m_speedV0 = speedV0;
    m_speedMax = speedMax;
    m_speedAcc = acc;
    m_speedUpMs = ms;
}

void Simple_BLDC_Motor::ConfigSpeedIndex(int speedIndex)
{
    m_speedIndex = speedIndex;
}

void Simple_BLDC_Motor::MoveAngle(float angle)
{
    MoveInit();
    m_dstAngle += (angle * m_coilPair);
}

void Simple_BLDC_Motor::MoveAngleTo(float angle)
{
    MoveInit();
    m_dstAngle = (angle * m_coilPair);
}

void Simple_BLDC_Motor::MoveIndex(int index)
{
    MoveInit();
    m_dstIndex = index;
}

void Simple_BLDC_Motor::MoveIndexTo(int index)
{
    MoveInit();
    m_dstIndex += index;
}

bool Simple_BLDC_Motor::IsMoving()
{
    return m_isMoving;
}

void Simple_BLDC_Motor::WaitMoveOver()
{
    while (m_isMoving)
    {
        Thread::Sleep(10);
    }
}

void Simple_BLDC_Motor::Start()
{
    m_spwma.Start();
    m_spwmb.Start();
    m_spwmc.Start();
}

void Simple_BLDC_Motor::Pause()
{
    m_isPause = true;
}

void Simple_BLDC_Motor::Stop()
{
    m_spwma.Stop();
    m_spwmb.Stop();
    m_spwmc.Stop();
}

SPwm &Simple_BLDC_Motor::GetSpwmA()
{
    return m_spwma;
}

SPwm &Simple_BLDC_Motor::GetSpwmB()
{
    return m_spwmb;
}

SPwm &Simple_BLDC_Motor::GetSpwmC()
{
    return m_spwmc;
}

void Simple_BLDC_Motor::Update()
{
#if false 
    if(m_isPause)
        return;

    if(m_speedMode == SpwmSpeedMode::Angle)
    {
        UpdateAngle();
    }
    else
    {
        UpdateIndex();
    }
#else 
    UpdateAngle();
#endif
}

void Simple_BLDC_Motor::SetAngle(float angle)
{
    m_spwma.SetAngle(angle);
    m_spwmb.SetAngle(angle + SPWM_PHASE_DETA);
    m_spwmc.SetAngle(angle + SPWM_PHASE_TWO_DETA);

    m_spwma.UpdateToPwm();
    m_spwmb.UpdateToPwm();
    m_spwmc.UpdateToPwm();
}

void Simple_BLDC_Motor::SetIndex(int index)
{
    m_spwma.SetIndex(index);
    m_spwmb.SetIndex(index + SPWM_PHASE_INDEX_DETA);
    m_spwmc.SetIndex(index + SPWM_PHASE_TWO_INDEX_DETA);

    m_spwma.UpdateToPwm();
    m_spwmb.UpdateToPwm();
    m_spwmc.UpdateToPwm();
}

void Simple_BLDC_Motor::UpdateAngle()
{
    if(m_isMoving == false)
        return;
        
    float deta = m_dstAngle - m_currentAngle;
    float inc = deta > 0? m_currentSpeedAngle : -m_currentSpeedAngle;
    float detaAbs = fabs(deta);
 
    if(detaAbs <= m_currentSpeedAngle || DETA_IS_ZERO(detaAbs)) //已经接近/等于目标值了
    {
        m_currentAngle = m_dstAngle;
        // m_controlState = MotorControlState::Idle;
        m_isMoving = false;
    }
    else
    {
        m_currentAngle += inc;
    }
    
    SetAngle(m_currentAngle);
}

void Simple_BLDC_Motor::UpdateIndex()
{
    int deta = m_dstIndex - m_currentIndex;
    int inc = deta > 0? m_speedIndex : -m_speedIndex;

    if(fabs(deta) < m_speedIndex) //已经接近/等于目标值了
    {
        m_currentIndex = m_dstIndex;
        m_isMoving = false;
    }
    else
    {
        m_currentIndex += inc;
    }
    
    SetIndex(m_currentIndex);
}

void Simple_BLDC_Motor::MoveInit()
{
    m_speedUpTotalAngle = 0;
    if(!m_isMoving)
        m_currentSpeedAngle = m_speedV0; 
    // m_accAnglePerUs = 

    m_controlState = MotorControlState::SpeedUp;
    m_isPause = false;
    m_isMoving = true;
}

void Simple_BLDC_Motor::SpeedControl()
{
    switch(m_controlState)
    {
        case MotorControlState::Idle:
        break;
        case MotorControlState::SpeedUp:
        {       
            if(m_currentSpeedAngle < m_speedMax)
            {
                m_currentSpeedAngle += m_speedAcc;
                m_speedUpTotalAngle += m_currentSpeedAngle;
            }

            if(m_currentSpeedAngle > m_speedMax)
            {
                m_currentSpeedAngle = m_speedMax;
                m_controlState = MotorControlState::SpeedMax;
            }
        }
        break;
        case MotorControlState::SpeedMax:
        {
            // if(detaAbs < m_speedUpTotalAngle)
            // {
            //     m_controlState = MotorControlState::SpeedDown;
            // }
        }
        break;
        case MotorControlState::SpeedDown:
        {
            if(m_currentSpeedAngle > m_speedV0)
            {
                // m_currentSpeedAngle -= m_speedAngleAcc;
                // if(m_currentSpeedAngle < m_speedAngleV0)
                // {
                //     m_currentSpeedAngle = m_speedAngleV0;
                //     m_controlState = MotorControlState::SpeedLow;
                // }
            }
        }
        case MotorControlState::SpeedLow:
        {
            
        }
        break;
    }
}
